#include "SteeringSeek.h"

#include <cassert>

#include "ai/steering/SteeringOutput.h"
#include "math/Vector3.h"
#include "physic/PhysicConstants.h"

SeekData::SeekData(const Point3* srcPos /*= nullptr*/,
                   const Point3* targetPos /*= nullptr*/,
                   const float speed /*= 0.0f*/)
                   : mSrcPos(srcPos)
                   , mTargetPos(targetPos)
                   , mSpeed(speed)
{

}

void SeekData::set(const Point3& srcPos,
         const Point3& targetPos,
         const float speed)
{
    mSrcPos = &srcPos;
    mTargetPos = &targetPos;
    mSpeed = speed;
}

void seek(const SeekData * const data, 
          SteeringOutput * const outputs,
          const uint32_t numData)
{
    assert(data);
    assert(outputs);
    assert(numData > 0);

    // PARALLEL_FOR
    for (size_t i = 0; i < numData; ++i) {
        const Point3& srcPos = *data[i].mSrcPos;
        const Point3& targetPos = *data[i].mTargetPos;
        const float speed = data[i].mSpeed;
        SteeringOutput& output = outputs[i];

        // Get the direction to the target
        output.mLinearVel.set(srcPos, targetPos);

        // The linear velocity is along previous direction
        // with the given speed
        normalize(output.mLinearVel, output.mLinearVel);
        output.mLinearVel *= speed;

        // Clamp linear velocity to max acceleration if necessary
        {
            const float sqrLen = output.mLinearVel.sqrLength();
            if (MAX_SPEED * MAX_SPEED < sqrLen) {
                output.mLinearVel.setLength(MAX_SPEED);
            }
        }
    }
}